COMMENT ⊗   VALID 00008 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	.TITLE MOVE
C00004 00003	ROUTINE TO EXECUTE A MOVE WITH NO 'AL ROUTINES
C00008 00004	ROUTINE TO DISPLAY CURRENT JOINT ANGLES
C00012 00005	ROUTINE TO DISPLAY CURRENT REFERENCE POWER SUPPLY READINGS
C00014 00006	FORCE SENSING/COMPLIANCE PATCH AREA
C00016 00007	SIMULTANEOUS HAND OPERATION
C00018 00008	LOCAL STORAGE AREA
C00021 ENDMK
C⊗;
.TITLE MOVE

TRANS ==1	;SET TO 1 TO PRINT OUT EULER AS WELL AS JOINT ANGLES
HANDOP==0	;=1 FOR SIMULTANEOUS HAND OPERATION

SNGSTP==0	;CONDITIONAL ASSEMBLY FLAGS FOR ARM.PAL
DIAGY ==0
STDALN==1
TACCAL==0
NOYELW==1
TIMER ==0
ISLIN ==1
FRCDAT==0

.INSRT K1DEF.PAL[11,SYS]
.INSRT ALHEAD.PAL[AL,HE]
.INSRT ARM.PAL[AL,HE]
.INSRT BEJCZY.PAL[AL,HE]
.INSRT ARITH.PAL[AL,HE]
.INSRT IO.PAL[3,BES]
.IFNZ TRANS
   .INSRT ARMSOL.PAL[al,he]
   .INSRT EULER.PAL[AL,HE]
.IFF
   UPDATE:
   SOLVE:	0
.ENDC


;COMMUNICATION LINK FOR "TLKEF5", "WHERE" ROUTINE FOR THE PDP10

MASLC5==110100	;ADDRESS OF MASTER NUMBER TO SIGNAL PDP10 WE ARE ALIVE
TRNADR==110102	;SAVE TRANSFORM DATA IN HERE
JTSADR==110162	;SAVE JOINT ANGLES IN HERE

MASTR5==12121	;CHECK NUMBER TO PDP10

;COMMUNICATION LOCATION BETWEEN MOVE.SAI AND MOVE.PAL

MCLOC==	100000

;DEVICE MECHANISM BITS

YELARM== 1	;MECHANISM BITS: YELLOW ARM, NOT INCLUDING HAND
YELHND== 2 	;YELLOW HAND
BLUARM== 4	;BLUE ARM, NOT INCLUDING HAND
BLUHND== 10	;BLUE HAND
;ROUTINE TO EXECUTE A MOVE WITH NO 'AL ROUTINES

P:	CLR	PRTMES		;DONT PRINT SERVO DATA BLOCK
	CLR	JMPFRC		;NO FORCE COMPLIANCE
	BR	STRT4

M:	MOV	#1,PRTMES	;PRINT SERVO ERROR MESSAGES
	MOV	#ASKFRC,SG	;CHECK IF FORCE COMPLIANCE WANTED
	JSR	PC,TYPSTR
	MOV	#IOBUF,SG
	JSR	PC,INSTR
	MOV	#IOBUF,SG
	JSR	PC,INTSCN
	MOV	R0,JMPFRC

STRT4:	MOV	#1,PRTHDR	;W HEADER NO LONGER ON VT05
	MOV	#START4,JOBSA
	JMP	1000

START4:	MOV	#DEVICE,R1	;INITIALIZE BLUE ARM TO RUN
	JSR	PC,INTARM
	TST	R0		;CHECK FOR ERRORS
	BEQ	MLOOP		;BRANCH IF NO INITIALIZATION ERROR
BADINT:	MOV	#BADSTR,SG	;ELSE TYPE ERROR MESSAGE
TYPERR:	JSR	PC,TYPSTR
	MOV	#IOBUF,SG	;TYPE ERROR CODE
	JSR	PC,CVO
	MOV	#IOBUF,SG
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	CLR	MASLC5		;INDICATE W NOT WORKING
	DISMIS

;WAIT LOOP LOOKING AT COMMAND BLOCK FROM PDP10

MLOOP: 	MOV	#12345,MCLOC    ;INDICATE ELF READY FOR DATA BLOCK
MWTLP:
   .IFNZ HANDOP
	TST	HANDGO		;HAND STILL GOING?
	BNE	1$		;YES
   .ENDC
	JSR	PC,PRTW		;PRINT THE WHERE LINE
1$:	TST	KBIS		;CHECK IF ANYONE HIT VT05 KEYBOARD
	BEQ	MMCHK 
	CLRB	KBIR
	CLR	MCLOC 		;INDICATE DIAG NO LONGER ACTIVE
	CLR	MASLC5		;INDICATE W NOT WORKING
	DISMIS			;EXIT TO DDT
MMCHK:	CMP	#54321,MCLOC	;CHECK IF DATA BLOCK ARRIVED
	BNE	MWTLP

;START OF MOVE CODE

      	MOV	#BARM+BHAND,WLST	;ELSE DO A WHERE FOR THE BLUE ARM
       	MOV	#WLST,R0
	MOV	#DEVICE,R1
	JSR	PC,WHERE
	TST	R0		;CHECK IF ANY ERRORS
	BEQ	OKWHER		;BRANCH IF OK
	MOV	#BADW,SG	;ELSE TYPE ERROR MESSAGE
	JMP	TYPERR
OKWHER:	TST	JMPFRC		;DO FORCE COMPLIANCE?
	BEQ	.+6
	JSR	PC,FPAT1
   .IFNZ HANDOP
	FORK	#HNDPDB,#SHAND
   .ENDC
	MOV	#MCLOC+4,R0	;POINT TO COEFFICIENT LIST
	MOV	#DEVICE,R1	;POINT TO DEVICE LIST
	CMP	#76,MCLOC+2	;MOVE OR CENTER?
	BNE	CENCOD
	JSR	PC,MOVE
	JMP	CENCD2
CENCOD:	JSR	PC,CENTER
CENCD2:	TST	JMPFRC		;CLEAR FORCE COMPLIANCE?
	BEQ	.+6
	JSR	PC,FPAT2
	TST	PRTMES		;TYPE OUT STATUS WORD?
	BEQ	MLOOP		;NO
	INC	PRTHDR		;INDICATE W HEADER OVERWRITTEN
	MOV	#COM6,SG	;TYPE OUT STATUS WORD
	JSR	PC,TYPSTR
	MOV	#IOBUF,SG
	JSR	PC,CVO
	MOV	#IOBUF,SG
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	MOV	DEVICE,R2	;GET THE NUMBER OF  SERVOS ATTACHED
	BEQ	ENDS4		;BRANCH IF NONE ATTACHED
	MOV	#COM7,SG	;ELSE TYPE OUT THE DEVICE BLOCK CONTENTS
	JSR	PC,TYPSTR
	MOV	#DEVICE+4,R1	;START WITH THIS WORD
DEVLP:	MOV	(R1)+,R0
	MOV	#IOBUF,SG
	JSR	PC,CVO
	MOV	#IOBUF,SG
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	SOB	R2,DEVLP
ENDS4:	JMP	MLOOP
;ROUTINE TO DISPLAY CURRENT JOINT ANGLES

W:	MOV	#START2,JOBSA
	JMP	1000

START2:	MOV	#1,PRTHDR	;PRINT THE WHERE HEADER
REDLP:	JSR	PC,PRTW
        TST	KBIS		;CHECK IF ANYONE HIT VT05 KEYBOARD
	BEQ	REDLP
	CLRB	KBIR
	CLR	MASLC5		;INDICATE "WHERE" NOT ACTIVE
	DISMIS

;START OF ROUTINE TO PRINT WHERE LINES

PRTW:	TST	PRTHDR		;NEED TO PRINT HEADER?
	BEQ	NOHDR
	MOV	#BARM+BHAND,WLST	;READ ALL BLUE ARM JOINTS
	MOV	#HEADER,SG	;TYPE OUT COLUMN HEADER
	JSR	PC,TYPSTR
	MOV	#8.,R0
	MOV	#2,R1
	JSR	PC,FORMAT
	CLR	PRTHDR
NOHDR: 	MOV	#WLST,R0	;POINT TO WHERE COEF. LIST
	MOV	#DEVICE,R1	;POINT TO DEVICE BLOCK
	JSR	PC,WHERE	;READ THE CURRENT JOINT ANGLES
	TST	R0		;CHECK FOR ERRORS
	BEQ	GOODW		;BRANCH IF OK
	MOV	#BADW,SG	;ELSE TYPE ERROR MESSAGE
	JMP	TYPERR
GOODW:	MOV	#COM4,SG	;POSITION CURSOR
	JSR	PC,TYPSTR
      	MOV	#IOBUF,SG	;START BUILDING OUTPUT STRING IN HERE
	MOV	#BTH,R2		;GET POINTERS JOINT READINGS
	MOV	#7,R3		;7 JOINTS IN ALL
JTCLP:	LDF 	@(R2)+,AC0	;GET JOINT ANGLE
	JSR	PC,CVF		;CONVERT JOINT ANGLE TO STRING 
	SOB	R3,JTCLP
	MOVB	#15,(SG)+	;PUT IN A CR LF
	MOVB	#12,(SG)+
	MOV	#9.,R0		;CHANGE STRING FORMAT FOR INTEGER NUMBER
	CLR	R1
	JSR	PC,FORMAT
	MOV	#BSRVOS,R1	;GET POINTERS TO SERVOS
	MOV	#7,R3		;CONVERT THE ACTUAL POT READINGS
NEXTP:	MOV	(R1)+,R0
	LDCIF	POT(R0),AC0	;GET A/D READING
	TST	POT(R0)		;CHECK IF ROUND UP OR DOWN
	BLT	RNDDN
	ADDF	#40000,AC0	;ROUND OFF READING
	BR	.+6
RNDDN:	SUBF	#40000,AC0
	JSR	PC,CVF		;CONVERT TO ASC
	SOB	R3,NEXTP
	JSR	PC,RSTFOR
	CLRB	(SG)
	MOV	#IOBUF,SG	;TYPE OUT WHOLE STRING
	JSR	PC,TYPSTR
   .IFNZ TRANS
	MOV	#TRNADR,R0	;GET CURRENT TRANSFORM IN HERE
	MOV	#THPTR,R1	;POINT TO THE CURRENT JOINT ANGLES
	MOV	#BLUARM,R2	;USE SOLUTION FOR BLUE ARM
	JSR	PC,UPDATE	;COMPUTE THE CURRENT TRANSFORM
	MOV	#TRNADR,R0	;COMPUTE THE EULER ANGLES
	MOV	#EANGLE,R1	;PUT THEM IN HERE
	JSR	PC,EULER
	MOV	#IOBUF,SG	;CONVERT TO STRING FOR OUTPUT
	MOVB	#15,(SG)+	;PUT IN A CR LF LF
	MOVB	#12,(SG)+
	MOVB	#12,(SG)+
	MOV	#6,R0
	MOV	#EANGLE,R1
EANGC:	LDF	(R1)+,AC0
	JSR	PC,CVF
	SOB	R0,EANGC	;REPEAT TILL DONE
	CLRB	(SG)
	MOV	#IOBUF,SG	;TYPE OUT THE EULER ANGLES
	JSR	PC,TYPSTR
	MOV	#BTH,R0		;TRANSFER THE JOINT ANGLES TO PDP10 
	MOV	#JTSADR,R1	;   COMMUNICATIONS TABLE
	MOV	#7,R2		;6 JOINTS + HAND
RHTLP:	LDF	@(R0)+,AC0	
	STF	AC0,(R1)+
	SOB	R2,RHTLP
	MOV	#MASTR5,MASLC5	;INDICATE TO PDP10 THAT "WHERE" IS WORKING
   .ENDC
	RTS	PC
;ROUTINE TO DISPLAY CURRENT REFERENCE POWER SUPPLY READINGS

REF:	MOV	#START3,JOBSA
	JMP	1000

START3:	MOV	#COM5,SG	;TYPE OUT THE CALIB. REFERENCE READING
	JSR	PC,TYPSTR
	MOV	REFER1,R0	;GET REF. READING
	MOV	#IOBUF,SG
	JSR	PC,CVI
	MOVB	#54,(SG)+	;PUT IN A COMMA AND SPACE CHARACTER
	MOVB	#40,(SG)+
	MOV	REFER2,R0	;GET SECOND REF. READING
	JSR	PC,CVI
	MOV	#IOBUF,SG	;TYPE THEM OUT
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	CLR	DEVICE		;DON'T READ ANY TACHS
REFLP:	MOV	#DEVICE,R1	;READ THE REF. POWER SUPPLY VOLTAGE
	JSR	PC,SETREF
      	MOV	#REFSG2,SG	;START BUILDING OUTPUT STRING IN HERE
	MOV	REFVT1,R0	;GET REFERENCE READINGS AND CONVERT TO ASC
	JSR	PC,CVI
	MOVB	#54,(SG)+	;PUT IN A COMMA AND SPACE CHARACTER
	MOVB	#40,(SG)+
	MOV	REFVT2,R0	;GET THE SECOND READING
	JSR	PC,CVI
	MOVB	#40,(SG)+	;PUT IN A FEW SPACE CHARACTERS
	MOVB	#40,(SG)+
	MOVB	#40,(SG)+
	MOV	#REFSTG,SG	;TYPE OUT WHOLE STRING
	JSR	PC,TYPSTR
        TST	KBIS		;CHECK IF ANYONE HIT VT05 KEYBOARD
	BEQ	REFLP
	CLRB	KBIR
	JSR	PC,CRLF
	DISMIS
;FORCE SENSING/COMPLIANCE PATCH AREA

FPAT1:	MOV	#FTABLE+BLUARM,R0;INIT FORCE SENSING SYSTEM
	MOV	#C,R1		;RELATIVE FORCE TRANS
	JSR	PC,SETC
	TST	R0		;ERROR?
	BEQ	1$
	BPT

.IFNZ 0
1$:	MOV	#BLUARM+XFORCE,R0
	LDF	FFORCE,AC0 
	JSR	PC,COMPLY
	TST	R0		;ERROR?
	BEQ	2$
	BPT
2$:	RTS	PC
	.BLKW	100

FPAT2:	RTS	PC
	.BLKW	100
.IFF
1$:	MOV	FBITS,R0
	MOV	#FPDB,R1
	MOV	#FSSUB,R2
	LDF	FFORCE,AC0
	JSR	PC,FRCSIG
	RTS	PC

	.BLKW	100

FPAT2:	MOV	FBITS,R0
	MOV	#FPDB,R1	
	JSR	PC,FRCOFF
	RTS	PC

	.BLKW	100

FSSUB:	MOV	#FMES,SG
	JSR	PC,TYPSTR
	DISMIS

FBITS:	BLUARM+XFORCE+SIGGE
FPDB:	PDBLK	3,30,FP
FMES:	.ASCIZ	/
πFORCE SENSING TRIGGERED
/
	.EVEN
.ENDC


FFORCE:	.FLT2	0.0

C:	.FLT2	1.0,0.0,0.0
	.FLT2	0.0,1.0,0.0
	.FLT2	0.0,0.0,1.0
	.FLT2	0.0,0.0,0.0
;SIMULTANEOUS HAND OPERATION

.IFNZ 	HANDOP

SHAND:	INC	HANDGO
	LDF	CI+SRV14,AC0	;PUT DYNAMIC COEF. IN DATA LIST
	LDF	CII+SRV14,AC1
	STF	AC0,HCI
	STF	AC1,HCII
	MOV	#HNDLST,R0	;HAND COEFFICIENT LIST
	MOV	#HDEVIC,R1
	JSR	PC,MOVE
	MOV	#HMES,SG	;TYPE STATUS CODE
	JSR	PC,TYPSTR
	MOV	#HIOBUF,SG
	JSR	PC,CVO
	MOV	#HIOBUF,SG
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	CLR	HANDGO
	DISMISS


;LOCAL STORAGE AREA

HNDPDB:	PDBLK	3,20,FP
HIOBUF:	.BLKW	100
HDEVIC:	.BLKW	10
HANDGO:	0
HNDLST:	BHAND
	0
	0		;NO SPECIAL MODES
	0		;NO WOBBLE
	40.		;RELATIVE SEGMENT POINTER
	5000.		;MOTION TIME
	0		;NO ASSOCIATED TRANS
	0		;NO RUN CODE
	.FLT2	0.0
	.FLT2	0.0
	.FLT2	0.0
	.FLT2	20.0
	.FLT2	-30.0
	.FLT2	12.0
HCI:	.FLT2	0.0	;CI
HCII:	.FLT2	0.0	;CII
	0		;NO OTHER SEGMENTS

HMES:	.ASCIZ	/
HAND FINISHED, STATUS = /
	.EVEN

.ENDC
;LOCAL STORAGE AREA

DEVICE:	.BLKW	33	;DEVICE BLOCK
IOBUF:	.BLKW	400.
EANGLE:	.BLKW	12.
PRTMES:	0
PRTHDR:	0
JMPFRC:	0

;WHERE COEFFICIENT DATA LIST

WLST:	374     ;JOINT SERVO BIT
	0

;OUTPUT STRINGS

COM4:	.BYTE	15,32,10,10,10
   .IFNZ TRANS
	.BYTE	32,10,10,10,32,10,10,10
   .ENDC
	.BYTE	0
HEADER:	.ASCII	/
   JT 1    JT 2    JT 3    JT 4    JT 5    JT 6     HAND

/
   .IFNZ TRANS
        .ASCII  /
     X       Y       Z       O       A       T
/
   .ENDC
	.BYTE	0
BADW:	.ASCIZ	/
SERVO ERROR WHILE ATTEMPTING TO DO A "WHERE", ERROR CODE = /
BADSTR:	.ASCIZ	/
ARM INITIALIZATION FAILED, ERROR CODE = /
COM5:	.ASCIZ	/
REF. POWER SUPPLY READINGS DURING CALIBRATION = /
REFSTG:	.ASCII	/CURRENT REFERENCE READINGS = /
REFSG2:	.ASCII	/                /
	.BYTE	15,10,10,10,0
COM6:	.ASCIZ	/

STATUS WORD = /
COM7:	.ASCIZ	/THE DEVICE BLOCK SERVO WORDS ARE AS FOLLOWS:
/
EVMES:	.ASCII	/TOUCH SENSOR EVENT TRIGGERED
/
	.BYTE	7,0
ASKFRC:	.ASCIZ	/
DO FORCE COMPLIANCE (0,1) = /
.EVEN

;MAIN PROGRAM PROCESSOR DESCRIPTOR BLOCK

MAINL:	PDBLK	1,100,FP
	
TEMP == .		
. = JOBDAT		;MAIN PROGRAM DESCRIPTOR BLOCK
	MAINL        
. = JOBSA		;START OF MAIN PROGRAM
	START2
. = JOBDM		;JOB DATA AREA MEMORY MAP
	USRDM
. = TEMP		;RESTORE LOCATION COUNTER

.END P